/********************************************************************************
*   
*    Copyright (C) GREUAH: http://www.roboticaeducativa.es
*    Depecabot 3.0 project depecabot@depeca.uah.es
*
*********************************************************************************
*
*    This file is part of depecabot3 libs.
*
*    Depecabot3 libs is free software: you can redistribute it and/or modify
*    it under the terms of the GNU General Public License as published by
*    the Free Software Foundation, either version 3 of the License, or
*    (at your option) any later version.
*
*    Depecabot3 libs is distributed in the hope that it will be useful,
*    but WITHOUT ANY WARRANTY; without even the implied warranty of
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*    GNU General Public License for more details.
*
*    For a copy of the GNU General Public License see 
*    <http://www.gnu.org/licenses/>
*
********************************************************************************/

#include "Depecabot3.h"

#include <Arduino.h>
#include <Servo.h>
#include "PCD8544.h"
#include "wiring_private.h"

#define MAX_GP2D120 	350 	//rango maximo del sensor en mm
#define MIN_GP2D120 	40 	//rango minimo del sensor en mm
#define MAX_GP2Y021 	800 	//rango maximo del sensor en mm
#define MIN_GP2Y021 	100 	//rango minimo del sensor en mm

const unsigned char logo_depeca [] = {		//Logo para la pantalla de bienvenida
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x0F, 0x03, 0xC3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3,
0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0x07, 0x00, 0xC0, 0xF8, 0xFF, 0x7F, 0x1F, 0x1F, 0x9F, 0x9F, 0x9F, 0x9F, 0x9F,
0x9F, 0x9F, 0x9F, 0x9F, 0x9F, 0x1F, 0x1F, 0x9F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x3F, 0x07,
0x00, 0xC0, 0xFC, 0xFF, 0x7F, 0x03, 0x00, 0xC0, 0xF8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0x3F, 0x07, 0x00, 0x80, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F,
0x01, 0x01, 0xE1, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0x39, 0x01, 0x06, 0xE6, 0xF8, 0xF9,
0x39, 0x01, 0x06, 0xF6, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF8, 0xF8,
0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xCF, 0xC0, 0xC0, 0xCC, 0xCF,
0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xC1, 0xC0, 0xE0, 0xFE, 0xFF, 0xFF, 0xC3, 0xC0, 0xC0,
0xCE, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
};

const unsigned char low_battery [] = {		//Icono de batería baja
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0xF0, 0xFC, 0x38, 0x08, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF8, 0x08, 0x08, 0x08, 0xFE, 0x02, 0x02, 0x02, 0x02, 0x02,
0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0xC2, 0xF2, 0xFE, 0x3F, 0x0F, 0x03, 0x03, 0x02, 0x02,
0x02, 0x02, 0xC2, 0xF2, 0x02, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x10, 0x10, 0x10, 0x7F, 0x40,
0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0xC0, 0xF0, 0xFC, 0x7F, 0x4F, 0x43, 0x40, 0x40, 0x40,
0x40, 0x40, 0x40, 0x50, 0x5C, 0x5F, 0x5F, 0x5F, 0x40, 0x7F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x1C, 0x3F, 0x0F, 0x03, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};



unsigned long encoder1Count,encoder2Count;	//Variables para el control de los encoders

unsigned char encoder1Value,encoder2Value,encoder1OldValue,encoder2OldValue;

Depecarduino::Depecarduino():encoder1(ENCODER1),encoder2(ENCODER2)	//Inicializacion de objetos en el constructor.
{

}

void Depecarduino::init ()	//Metodo de inicializacion del robot
{
	float bat;

	Serial1.begin(115200);	//Puerto serie bluetooth
	Wire.begin();		//I2C
	encoderInit();		//Configuracion de la interrupcion

	pinMode(MI_A,OUTPUT);	//Configuracion de los pines de los motores
	pinMode(MI_B,OUTPUT);
	pinMode(MD_A,OUTPUT);
	pinMode(MD_B,OUTPUT);
	
	this->lcd.begin(84, 48);	//Configuracion del LCD

	bat=bateria();		//Lee el nivel de bateria

	if(bat>6.0)		//Si la bateria esta bien continua
	{
		this->lcd.setCursor(20, 0);	//Pantalla de bienvenida
		this->lcd.drawBitmap(logo_depeca, 46, 5);

		this->lcd.setCursor(0, 5);
		this->lcd.print("Depecabot 3.0");
		delay(1000);
	}
	else
	{
		this->lcd.setCursor(24, 0);	//Bateria baja
		this->lcd.drawBitmap(low_battery, 36, 4);

		this->lcd.setCursor(28, 4);
						//Si la bateria es baja espera confirmacion del usuario para empezar
	  	this->lcd.print(bat);		//Y visualiza el voltaje
		this->lcd.print("V");
		this->lcd.setCursor(0, 5);

		this->lcd.print("P1 Continuar");
		while(!pulsador(1));
	}
}

unsigned char Depecarduino::pulsador(unsigned char canal)	//Devuelve el estado del pulsador especificado
{
	unsigned char puerto_pcf,salida;
	
	if(canal>3)
		salida=0;
	else
		if(canal==1)
			salida=~digitalRead(P1)&0x01;	//Negamos
		else
			if(canal==2)
				salida=~digitalRead(P2)&0x01;	//Negamos
			else
				if(canal==3)
					salida=~digitalRead(P3)&0x01;	//Negamos
	return salida;
}

void Depecarduino::motores(int velocidad1,int velocidad2)	//Controla los motores individualmente
{
	if(velocidad1>255)		//Saturacion
		velocidad1=255;
	if(velocidad1<-255)
		velocidad1=-255;
	if(velocidad2>255)
		velocidad2=255;
	if(velocidad2<-255)
		velocidad2=-255;
		
	if(velocidad1>=0)		//Si la velocidad es positiva escribe en la rama A
	{
		analogWrite(MI_A,velocidad1);
		analogWrite(MI_B,0);
	}
	else		//Si la velocidad es negativa escribe en la rama B
	{
		analogWrite(MI_A,0);
		analogWrite(MI_B,-velocidad1);
	}

//Los pines que controlan el motor derecho: 9 y 10 no se pueden usar a la vez que el servo 
//porque son controlados por el timer 1, el mismo que usa la libreria servo
//Sin embargo también están conectados a las salidas del módulo comparación B del timer 4
//Estas salidas son complementarias y habrá que usar solo la directa o la negada y anular la otra

	if(velocidad2>=0)		//Si la velocidad es positiva escribe en la rama A
	{
		cbi(TCCR1A, COM1A1);	//Desconecta el pin 9 del timer 1
		cbi(TCCR1A, COM1A0);
		sbi(TCCR1A, COM1B1);	//Conecta el pin 10 al timer 1 para que no le afecte el timer 4
		cbi(TCCR1A, COM1B0);

		sbi(TCCR4A, PWM4B);	//Habilita modo PWM en el comparador B del timer 4

		sbi(TCCR4A, COM4B0);	//Habilita las dos salidas del comparador B
		cbi(TCCR4A, COM4B1);

		sbi(TCCR4B, PWM4X);	//El pin 9 es la salida invertida, activamos la inversión para que salga directo

		OCR4B = (unsigned char) velocidad2;
	}
	else		//Si la velocidad es negativa escribe en la rama B
	{
		sbi(TCCR1A, COM1A1);	//Conecta el pin 9 al timer 1 para que no le afecte el timer 4
		cbi(TCCR1A, COM1A0);
		cbi(TCCR1A, COM1B1);	//Desconecta el pin 10 del timer 1
		cbi(TCCR1A, COM1B0);

		sbi(TCCR4A, PWM4B);	//Habilita modo PWM en el comparador B del timer 4

		sbi(TCCR4A, COM4B0);	//Habilita las dos salidas del comparador B
		cbi(TCCR4A, COM4B1);

		cbi(TCCR4B, PWM4X);	//El pin 10 es la salida no invertida, desactivamos la inversión

		OCR4B = (unsigned char) -velocidad2;
	}
}

void Depecarduino::motoresLinealAngular(int velocidadlineal,int velocidadangular)//Control de los motores en velocidad lineal y angular
{
	int velocidad1,velocidad2;
	
	velocidad1=velocidadlineal+velocidadangular;	//Calcula las velocidades individuales
	velocidad2=velocidadlineal-velocidadangular;
	
	motores(velocidad1,velocidad2);	//Las escribe en los motores
}

Encoder::Encoder(unsigned char encoderpin)	//Constructor del objeto encoder
{
	this->pin=encoderpin;
}
	
unsigned int Encoder::read()		//Lectura del encoder, las variables son externas, solo enmascara en forma de objeto para dar coherencia.
{
	if(this->pin==ENCODER1)
		return encoder1Count;
	if(this->pin==ENCODER2)
		return encoder2Count;
}

void Encoder::reset()			//Reinicio del encoder
{
	if(this->pin==ENCODER1)
		encoder1Count=0;
	if(this->pin==ENCODER2)
		encoder2Count=0;
}


SIGNAL(PCINT0_vect)	//Interrupción de cambio de nivel
{
	encoder1Value=digitalRead(ENCODER1);
	encoder2Value=digitalRead(ENCODER2);	//Lee los valores actuales de los pines

	if(encoder1Value!=encoder1OldValue)	//Si el pin ha cambiado de estado incrementa su contador
	{
		encoder1Count++;
		encoder1OldValue=encoder1Value;
	}
	if(encoder2Value!=encoder2OldValue)	//Si el pin ha cambiado de estado incrementa su contador
	{
		encoder2Count++;
		encoder2OldValue=encoder2Value;
	}
}

void encoderInit()
{
  PCMSK0 |= 0x01 << 4;	//Habilita interrupción en el pin 8 de arduino
  PCMSK0 |= 0x01 << 7;	//Habilita interrupción en el pin 11 de arduino
  PCICR |= 0x01;	//Habilita interrupcion en cambio de nivel
}

//Calcula la tensión VRef real, leyendo la referencia interna de 1.1V
//http://hacking.majenko.co.uk/making-accurate-adc-readings-on-arduino

long readVcc(void) 
{ 
  long result; // Read 1.1V reference against AVcc 

  ADCSRB &= ~(1 << MUX5);
  ADMUX=0x5E;		//For ATMEGA32U4

  delayMicroseconds(300); // Wait for Vref to settle 

  ADCSRA |= _BV(ADSC); // Convert 
  while (bit_is_set(ADCSRA,ADSC)); 
  result = ADCL; 
  result |= ADCH<<8; 
  result = 1125300L / result; // Back-calculate AVcc in mV 
  return result; 
}

int analogReadComp(int canal)	//Lectura compensada del canal ADC deseado
{
  int adc,vref,adcreal;
  adc=analogRead(canal);

  vref=readVcc();		//Obtiene el valor real de la tension de referencia
  adcreal=((float)vref/5000)*adc;	//Lo escala a una referencia de 5V

  return adcreal;
}

float Depecarduino::bateria(void)
{
 	float bateria;

	bateria=0.0223*(float)analogReadComp(VBAT);		//A 8.0V el ADC lee 255
	
	return bateria;
}

unsigned int Depecarduino::gp2ymm(unsigned char channel)	//Lectura de sensores de distancia tipo GP2Y021
{
	unsigned int valor;
	float distancia;

  	valor=analogReadComp(channel);

  	distancia = (35388 / (valor - 65.7)) + 3.38; // Fórmula de conversión de tensión a mm, linealizada

	if(distancia<0)
   		distancia = 800; //depuracion valores negativos 
  	else if (distancia > 800)
    		distancia = 800; //saturamos
  	else if (distancia < 70)	//Aunque el fabricante especifica un rango minimo de 100mm el sensor lee bien hasta 70mm
    		distancia = 70; //saturamos 
	
	return (int)distancia;
}

unsigned int Depecarduino::gp2dmm(unsigned char channel)	//Lectura de sensores de distancia tipo GP2D120
{
 	float distancia;

	distancia = 10*((2914.0 / (analogReadComp(channel) + 5.0) - 1));//Formula de conversion de tension a mm.
	
	if (distancia>MAX_GP2D120)
		distancia=MAX_GP2D120; //saturamos
	else if (distancia<MIN_GP2D120)
			distancia=MIN_GP2D120; //saturamos
	
	return (int)distancia;
}

//Las siguientes funciones permiten escribir el LCD pixel por pixel.
//Al no poder leer el contenido de un byte de la memoria del LCD, es necesario crear el mapa de bits en memoria para poder modificarlo.

void PCD8544_pixel::writePixel(unsigned char x, unsigned char y, unsigned char value)
{
	int byte_pixel;		//Posicion a escribir

	if(this->mapa==NULL)	//Si nunca se ha escrito el puntero no estara inicializado
	{
		this->mapa=(unsigned char *)malloc(504);	//Crea el array dinamicamente para ahorrar memoria cuando no se usa

		for(byte_pixel=0;byte_pixel<504;byte_pixel++)
		{
			this->mapa[byte_pixel]=0;	//Limpia el mapa de bits de basura
		}
	}

	byte_pixel=x+84*(y/8); //Calcula la posicion de memoria que contiene al pixel solicitado

	if(value)
		this->mapa[byte_pixel]|=0x01<<(y%8);	//Para activar un pixel escribe a 1 el bit correspondiente
	else
		this->mapa[byte_pixel]&=~0x01<<(y%8);	//Para borrar un pixel escribe a 0 el bit correspondiente
}

void PCD8544_pixel::refreshPixelMap(void)	//Vuelca el mapa de bits en memoria al display, bloqueante, tarda unos 40ms aproximadamente
{
	int byte_pixel;
	this->setCursor(0,0);	//Empieza en el primer byte

	digitalWrite(LCD_DC, HIGH);	//Modo datos
	digitalWrite(LCD_SCE, LOW);	//Seleccion de la pantalla en el bus

	for(byte_pixel=0;byte_pixel<504;byte_pixel++)
	{
		SPDR = this->mapa[byte_pixel];	//Escribe cada byte en el LCD consecutivamente
		while (!(SPSR & (1 << SPIF)));	//Utilizando SPI hardware
	}

	digitalWrite(LCD_SCE, HIGH);	//Libera la pantalla
}

